#include "protocol.h"

protocol::protocol()
{
    // 清空变量
    memset(&data_dipan_pc_heart         ,0, sizeof(data_dipan_pc_heart));
    memset(&data_dipan_pc_ult           ,0, sizeof(data_dipan_pc_ult));
    memset(&data_dipan_pc_other         ,0, sizeof(data_dipan_pc_other));
    memset(&data_dipan_pc_encoder       ,0, sizeof(data_dipan_pc_encoder));
    memset(&data_dipan_pc_speed         ,0, sizeof(data_dipan_pc_speed));
    memset(&data_dipan_pc_reply         ,0, sizeof(data_dipan_pc_reply));
    memset(&data_dipan_pc_imu           ,0, sizeof(data_dipan_pc_imu));
    memset(&data_dipan_pc_status        ,0, sizeof(data_dipan_pc_status));
    memset(&data_dipan_pc_alarm         ,0, sizeof(data_dipan_pc_alarm));

    memset(&data_pc_dipan_switch        ,0, sizeof(data_pc_dipan_switch));
    memset(&data_pc_dipan_ackerman      ,0, sizeof(data_pc_dipan_ackerman));
    memset(&data_pc_dipan_reset_odom    ,0, sizeof(data_pc_dipan_reset_odom));
    memset(&data_pc_dipan_fine_control  ,0, sizeof(data_pc_dipan_fine_control));
    memset(&data_pc_dipan_motion_mode   ,0, sizeof(data_pc_dipan_motion_mode));
    memset(&data_pc_dipan_cmd           ,0, sizeof(data_pc_dipan_cmd));
    memset(&data_pc_dipan_time_sync     ,0, sizeof(data_pc_dipan_time_sync));
    memset(&data_pc_dipan_heart         ,0, sizeof(data_pc_dipan_heart));
    memset(&data_pc_dipan_steer_engine  ,0, sizeof(data_pc_dipan_steer_engine));

    memset(&data_dianyuan_pc_heart      ,0, sizeof(data_dianyuan_pc_heart));
    memset(&data_dianyuan_pc_battery    ,0, sizeof(data_dianyuan_pc_battery));
    memset(&data_dianyuan_pc_status     ,0, sizeof(data_dianyuan_pc_status));
    memset(&data_dianyuan_pc_reply      ,0, sizeof(data_dianyuan_pc_reply));
    memset(&data_dianyuan_pc_radar      ,0, sizeof(data_dianyuan_pc_radar));
    memset(&data_dianyuan_pc_radar_f    ,0, sizeof(data_dianyuan_pc_radar_f));
    memset(&data_dianyuan_pc_radar_b    ,0, sizeof(data_dianyuan_pc_radar_b));
    memset(&data_dianyuan_pc_func_flag  ,0, sizeof(data_dianyuan_pc_func_flag));
    memset(&data_dianyuan_pc_alarm      ,0, sizeof(data_dianyuan_pc_alarm));

    memset(&data_pc_dianyuan_switch     ,0, sizeof(data_pc_dianyuan_switch));
    memset(&data_pc_dianyuan_state      ,0, sizeof(data_pc_dianyuan_state));
    memset(&data_pc_dianyuan_time_sync  ,0, sizeof(data_pc_dianyuan_time_sync));
    memset(&data_pc_dianyuan_heart      ,0, sizeof(data_pc_dianyuan_heart));
    memset(&data_pc_dianyuan_func_set   ,0, sizeof(data_pc_dianyuan_func_set));
    memset(&data_pc_dianyuan_charge_id  ,0, sizeof(data_pc_dianyuan_charge_id));

    memset(frequency_count, 0, sizeof(frequency_count));
    memset(frequency, 0, sizeof(frequency));
}

protocol::~protocol()
{

}

void protocol::decode(uint8_t *data, uint16_t type, int size)
{
    uint16_t typelength = protocol_get_type_length((protocol_type)type);
    if(size != typelength)
    {
        qDebug() << "长度有误" << "type = " << type << "length = "<< typelength;
        return;
    }
    switch (type)
    {
        case type_dipan_pc_heart         :
            memcpy(&data_dipan_pc_heart, data, size);
            frequency_count[type_dipan_pc_heart]++;
        break;

        case type_dipan_pc_ult           :
            memcpy(&data_dipan_pc_ult, data, size);
            frequency_count[type_dipan_pc_ult]++;
        break;

        case type_dipan_pc_other         :
            memcpy(&data_dipan_pc_other, data, size);
            frequency_count[type_dipan_pc_other]++;
        break;

        case type_dipan_pc_encoder       :
            memcpy(&data_dipan_pc_encoder, data, size);
            frequency_count[type_dipan_pc_encoder]++;
        break;

        case type_dipan_pc_speed         :
            memcpy(&data_dipan_pc_speed, data, size);
            frequency_count[type_dipan_pc_speed]++;
        break;

        case type_dipan_pc_reply         :
            memcpy(&data_dipan_pc_reply, data, size);
            frequency_count[type_dipan_pc_reply]++;
        break;

        case type_dipan_pc_imu           :
            memcpy(&data_dipan_pc_imu, data, size);
            frequency_count[type_dipan_pc_imu]++;
        break;

        case type_dipan_pc_status        :
            memcpy(&data_dipan_pc_status, data, size);
            frequency_count[type_dipan_pc_status]++;
        break;

        case type_dipan_pc_alarm         :
            memcpy(&data_dipan_pc_alarm, data, size);
            frequency_count[type_dipan_pc_alarm]++;
        break;

        case type_dianyuan_pc_heart      :
            memcpy(&data_dianyuan_pc_heart, data, size);
            frequency_count[type_dianyuan_pc_heart]++;
        break;

        case type_dianyuan_pc_battery    :
            memcpy(&data_dianyuan_pc_battery, data, size);
            frequency_count[type_dianyuan_pc_battery]++;
        break;

        case type_dianyuan_pc_status     :
            memcpy(&data_dianyuan_pc_status, data, size);
            frequency_count[type_dianyuan_pc_status]++;
        break;

        case type_dianyuan_pc_reply      :
            memcpy(&data_dianyuan_pc_reply, data, size);
            frequency_count[type_dianyuan_pc_reply]++;
        break;

        case type_dianyuan_pc_radar      :
            memcpy(&data_dianyuan_pc_radar, data, size);
            frequency_count[type_dianyuan_pc_radar]++;
            if(data_dianyuan_pc_radar.radar_ID == 1) {
                data_dianyuan_pc_radar_f = data_dianyuan_pc_radar;
            } else if (data_dianyuan_pc_radar.radar_ID == 2) {
                data_dianyuan_pc_radar_b = data_dianyuan_pc_radar;
            } else {}
        break;

        case type_dianyuan_pc_func_flag  :
            memcpy(&data_dianyuan_pc_func_flag, data, size);
            frequency_count[type_dianyuan_pc_func_flag]++;
        break;

        case type_dianyuan_pc_alarm     :
            memcpy(&data_dianyuan_pc_alarm, data, size);
            frequency_count[type_dianyuan_pc_alarm]++;
        break;
    }
}

void protocol::unpack(QByteArray data)
{
    if(data.isEmpty())
    {
        return;
    }
    else
    {
//        if(data.at(0) != '\xff' || data.at(1) != '\xfe')
        if(data.size() < 10)
        {
            qDebug() << "长度错误";
            return;
        }
        if((uint8_t)data.at(0) != 0xff || (uint8_t)data.at(1) != 0xfe)
        {
            qDebug() << "帧头错误";
            return;
        }
        uint16_t type = ((uint8_t)data.at(2) + ((uint8_t)data.at(3) << 8));
        uint16_t size = ((uint8_t)data.at(4) + ((uint8_t)data.at(5) << 8));
        if(data.size() < size)
        {
            qDebug() << "长度错误";
            return;
        }
        if((uint16_t)(data.at(size - 2) & 0xFF) + (uint16_t)((data.at(size - 1) & 0xFF) << 8) != PACK_END)
        {
            qDebug() << "帧尾错误";
            qDebug() << data.toHex(' ');
            return;
        }
        decode((uint8_t*)data.data(), type, size);
        //递归调用
        unpack(data.remove(0, size));
    }
}

uint16_t protocol::protocol_get_type_length(protocol_type type)
{
    static uint16_t type_length_table[64] = {0};
    static uint8_t type_length_table_init_flag = 0;
    //对表进行初始化
    if(type_length_table_init_flag == 0)
    {
        // 底盘->上位机
        type_length_table[type_dipan_pc_heart]          = sizeof(protocol_dipan_pc_heart);
        type_length_table[type_dipan_pc_ult]            = sizeof(protocol_dipan_pc_ult);
        type_length_table[type_dipan_pc_other]          = sizeof(protocol_dipan_pc_other);
        type_length_table[type_dipan_pc_encoder]        = sizeof(protocol_dipan_pc_encoder);
        type_length_table[type_dipan_pc_speed]          = sizeof(protocol_dipan_pc_speed);
        type_length_table[type_dipan_pc_reply]          = sizeof(protocol_dipan_pc_reply);
        type_length_table[type_dipan_pc_imu]            = sizeof(protocol_dipan_pc_imu);
        type_length_table[type_dipan_pc_status]         = sizeof(protocol_dipan_pc_status);
        type_length_table[type_dipan_pc_alarm]          = sizeof(protocol_dipan_pc_alarm);
        // 上位机->底盘
        type_length_table[type_pc_dipan_switch]         = sizeof(protocol_pc_dipan_switch);
        type_length_table[type_pc_dipan_ackerman]       = sizeof(protocol_pc_dipan_ackerman);
        type_length_table[type_pc_dipan_reset_odom]     = sizeof(protocol_pc_dipan_reset_odom);
        type_length_table[type_pc_dipan_fine_control]   = sizeof(protocol_pc_dipan_fine_control);
        type_length_table[type_pc_dipan_motion_mode]    = sizeof(protocol_pc_dipan_motion_mode);
        type_length_table[type_pc_dipan_cmd]            = sizeof(protocol_pc_dipan_cmd);
        type_length_table[type_pc_dipan_time_sync]      = sizeof(protocol_pc_dipan_time_sync);
        type_length_table[type_pc_dipan_heart]          = sizeof(protocol_pc_dipan_heart);
        type_length_table[type_pc_dipan_steer_engine]   = sizeof(protocol_pc_dipan_steer_engine);
        // 电源板->上位机
        type_length_table[type_dianyuan_pc_heart]       = sizeof(protocol_dianyuan_pc_heart);
        type_length_table[type_dianyuan_pc_battery]     = sizeof(protocol_dianyuan_pc_battery);
        type_length_table[type_dianyuan_pc_status]      = sizeof(protocol_dianyuan_pc_status);
        type_length_table[type_dianyuan_pc_reply]       = sizeof(protocol_dianyuan_pc_reply);
        type_length_table[type_dianyuan_pc_radar]       = sizeof(protocol_dianyuan_pc_radar);
        type_length_table[type_dianyuan_pc_func_flag]   = sizeof(protocol_dianyuan_pc_func_flag);
        type_length_table[type_dianyuan_pc_alarm]       = sizeof(protocol_dianyuan_pc_alarm);
        // 上位机->电源板
        type_length_table[type_pc_dianyuan_switch]      = sizeof(protocol_pc_dianyuan_switch);
        type_length_table[type_pc_dianyuan_state]       = sizeof(protocol_pc_dianyuan_state);
        type_length_table[type_pc_dianyuan_time_sync]   = sizeof(protocol_pc_dianyuan_time_sync);
        type_length_table[type_pc_dianyuan_heart]       = sizeof(protocol_pc_dianyuan_heart);
        type_length_table[type_pc_dianyuan_func_set]    = sizeof(protocol_pc_dianyuan_func_set);
        type_length_table[type_pc_dianyuan_charge_id]   = sizeof(protocol_pc_dianyuan_charge_id);

        type_length_table_init_flag = 1;
    }
    return type_length_table[type];
}

protocol_head protocol::protocol_head_pack(protocol_type type, uint32_t time)
{
    protocol_head temp;
    temp.head = 0xff;
    temp.protocol_version = 0xfe;
    temp.protocol_type = type;
    temp.temp = 0x00;
    temp.data_length = protocol_get_type_length(type);
    temp.current_time = time;
    return temp;
}

QByteArray protocol::protocol_pc_dipan_switch_pcak(uint8_t type, uint8_t control)
{
    protocol_pc_dipan_switch temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_switch, 0);
    temp.switch_type = type;
    temp.switch_control = control;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_ackerman_pack(uint8_t mode, float v, float w, float r, float d)
{
    protocol_pc_dipan_ackerman temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_ackerman, 0);
    temp.mode = mode;
    temp.v = v;
    temp.w = w;
    temp.r = r;
    temp.deg = d;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_reset_odom_pack(float x, float y, float d)
{
    protocol_pc_dipan_reset_odom temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_reset_odom, 0);
    temp.x = x;
    temp.y = y;
    temp.yaw = d;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_fine_pack(float vlf, float vrf, float vlb, float vrb, float dlf, float drf, float dlb, float drb)
{
    protocol_pc_dipan_fine_control temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_fine_control, 0);
    temp.v_lf = vlf;
    temp.v_rf = vrf;
    temp.v_lb = vlb;
    temp.v_rb = vrb;
    temp.deg_lf = dlf;
    temp.deg_rf = drf;
    temp.deg_lb = dlb;
    temp.deg_rb = drb;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_motion_mode_pack(uint8_t mode)
{
    protocol_pc_dipan_motion_mode temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_motion_mode, 0);
    temp.mode = mode;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_cmd_pack(uint8_t cmd)
{
    protocol_pc_dipan_cmd temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_cmd, 0);
    temp.cmd = cmd;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_pc_dipan_time_sync_pack()
{
    protocol_pc_dipan_time_sync temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_time_sync, 0);
    temp.timestamp = 0;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_pc_dipan_heart_pack(uint8_t nav_pc, uint8_t img_pc)
{
    protocol_pc_dipan_heart temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_heart, 0);
    temp.navigation_pc_state = nav_pc;
    temp.image_pc_state = img_pc;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_pc_dipan_steer_engine_pack(uint8_t control)
{
    protocol_pc_dipan_steer_engine temp;
    temp.protocol_head = protocol_head_pack(type_pc_dipan_steer_engine, 0);
    temp.control = control;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_switch_pack(uint8_t type, uint8_t control)
{
    protocol_pc_dianyuan_switch temp;
    temp.protocol_head = protocol_head_pack(type_pc_dianyuan_switch, 0);
    temp.switch_type = type;
    temp.switch_control = control;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_state_pack(uint8_t state)
{
    protocol_pc_dianyuan_state temp;
    temp.protocol_head = protocol_head_pack(type_pc_dianyuan_state, 0);
    temp.state = state;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_pc_dianyuan_time_sync_pack()
{
    protocol_pc_dianyuan_time_sync temp;
    temp.protocol_head = protocol_head_pack(type_pc_dianyuan_time_sync, 0);
    temp.timestamp = 0;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_pc_dianyuan_heart_pack(uint8_t nav_pc, uint8_t img_pc)
{
    protocol_pc_dianyuan_heart temp;
    temp.protocol_head = protocol_head_pack(type_pc_dianyuan_heart, 0);
    temp.navigation_pc_state = nav_pc;
    temp.image_pc_state = img_pc;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_pc_dianyuan_func_set_pack(uint8_t id, uint8_t cmd)
{
    protocol_pc_dianyuan_func_set temp;
    temp.protocol_head = protocol_head_pack(type_pc_dianyuan_func_set, 0);
    temp.func_id = id;
    temp.func_cmd = cmd;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

QByteArray protocol::protocol_pc_dianyuan_charge_id_pack(uint8_t charge_id)
{
    protocol_pc_dianyuan_charge_id temp;
    temp.protocol_head = protocol_head_pack(type_pc_dianyuan_charge_id, 0);
    temp.charge_id = charge_id;
    temp.crc16 = crc16((uint8_t*)&temp, sizeof(temp) - 4, crc_16_MODBUS);
    temp.end = PACK_END;
    QByteArray data((char*)&temp, sizeof(temp));
    return data;
}

void protocol::update_frequency()
{
    for(int i = 0; i < SUM_ENUM; i++)
    {
        this->frequency[i] = frequency_count[i];
        frequency_count[i] = 0;
    }
}
